#include "orbbec_camera.hpp"

#include "spdlog/spdlog.h"

namespace Orbbec {

void GetDepthHistogram(cv::Mat& src, cv::Mat& dst) {
  float depthHistogram[65536];
  int numberOfPoints = 0;
  cv::Mat depthHist(src.rows, src.cols, CV_8UC3);
  memset(depthHistogram, 0, sizeof(depthHistogram));
  for (int y = 0; y < src.rows; ++y) {
    ushort* depthCell = (ushort*)src.ptr<uchar>(y);
    for (int x = 0; x < src.cols; ++x) {
      if (*depthCell != 0) {
        depthHistogram[*depthCell]++;
        numberOfPoints++;
      }
      depthCell++;
    }
  }

  for (std::size_t i = 1; i < sizeof(depthHistogram) / sizeof(int); i++) {
    depthHistogram[i] += depthHistogram[i - 1];
  }
  for (std::size_t i = 1; i < sizeof(depthHistogram) / sizeof(int); i++) {
    depthHistogram[i] = (numberOfPoints - depthHistogram[i]) / numberOfPoints;
  }
  for (int y = 0; y < src.rows; ++y) {
    ushort* depthCell = (ushort*)src.ptr<uchar>(y);
    uchar* showcell = (uchar*)depthHist.ptr<uchar>(y);
    for (int x = 0; x < src.cols; ++x) {
      char depthValue = depthHistogram[*depthCell] * 255;
      *showcell++ = 0;
      *showcell++ = depthValue;
      *showcell++ = depthValue;

      depthCell++;
    }
  }
  dst = depthHist;
}

}  // namespace Orbbec

void OrbbecCamera::GrabPrepare() {}

void OrbbecCamera::GrabLoop() {
  cv::Mat cv_rgb(frame_h_, frame_w_, CV_8UC3);
  cv::Mat cv_depth(frame_h_, frame_w_, CV_16UC1);  // 640x480

  // 分配的应该为RGB的宽和高,保存D2C后的Depths
  cv::Mat aligned_depth(frame_h_, frame_w_, CV_16UC1);
  // 分配的内存和aligned_depth一样
  cv::Mat CaliDepthHistogram(frame_h_, frame_w_, CV_16UC1);

  WarnReport(camera_->GetStreamData(cv_rgb, cv_depth));
  cv::imshow("cv_color", cv_rgb);
  cv::imshow("cv_depth", cv_depth);
  if (type_ == SOFTWARE_D2C) {
    WarnReport(d2c_.D2C(cv_depth.ptr<uint16_t>(), frame_w_, frame_h_,
                        aligned_depth.ptr<uint16_t>(), frame_w_, frame_h_),
               "D2C failed!");
    if (mirror_ == Orbbec::COLOR_IS_MIRROR) {
      cv::flip(aligned_depth, aligned_depth, 1);
    }
    Orbbec::GetDepthHistogram(aligned_depth, CaliDepthHistogram);
  } else {
    if (mirror_ == Orbbec::COLOR_IS_MIRROR) {
      cv::flip(cv_depth, cv_depth, 1);
    }
    Orbbec::GetDepthHistogram(cv_depth, CaliDepthHistogram);
  }
  cv::imshow("dep", CaliDepthHistogram);
  std::lock_guard<std::mutex> lock(frame_stack_mutex_);
  frame_stack_.clear();
  frame_stack_.push_front(CaliDepthHistogram.clone());
  frame_signal_.Signal();
}

bool OrbbecCamera::OpenPrepare(unsigned int index) {
  (void)index;
  WarnReport(camera_->CameraInit(NORMAL_NO));

  //获取相机内外参
  SW_D2C soft_d2c;
  memset(&soft_d2c, 0, sizeof(SW_D2C));
  OBCameraParams cameraParams;
  memset(&cameraParams, 0, sizeof(OBCameraParams));

  WarnReport(camera_->GetCameraParam(cameraParams));

  //获取设备的分辨率
  int width = 0;
  int height = 0;
  WarnReport(camera_->GetCameraResolution(width, height));
  memcpy(&soft_d2c, &cameraParams, sizeof(SW_D2C));
  //调用D2C库 装载参数

  WarnReport(d2c_.LoadParameters((SW_D2C*)&soft_d2c), "LoadParameters failed!");

  WarnReport(d2c_.PrepareDepthResolution(width, height),
             "PrepareDepthResolution failed!");

  // 关闭畸变
  WarnReport(d2c_.EnableDistortion(false), "EnableDistortion failed!");

  // fill gaps
  WarnReport(d2c_.EnableDepthGapFill(true), "EnableDepthGapFill failed!");

  // 每个深度单位=1.0 mm
  WarnReport(d2c_.SetDepthUnit(1), "EnableDistortion failed!");

  WarnReport(d2c_.SetMirrorMode(true), "SetMirrorMode failed!");

  // 设置原始深度图中，感兴趣的深度范围，根据实际产品的深度范围设置，单位mm
  WarnReport(d2c_.SetDepthRange(300.0, 2900.0), "SetMirrorMode failed!");

  if (camera_->GetDevicePid() != Orbbec::kPID_0402)
    return false;
  else
    return true;
}

OrbbecCamera::OrbbecCamera() {
  SPDLOG_WARN(" *********** soft d2c v1.0*********");

  SPDLOG_WARN("Version: {}", d2c_.GetVersion());
  camera_ = new Astra();
  type_ = NORMAL_NO;
  mirror_ = true;
}

OrbbecCamera::~OrbbecCamera() {
  Close();
  if (camera_) {
    delete camera_;
    camera_ = NULL;
  }
}

int OrbbecCamera::Close() {
  grabing = false;
  grab_thread_.join();
  WarnReport(camera_->StreamStop());
  WarnReport(camera_->CameraUnInit());

  return 0;
}
